package myoar

import com.thalmic.{myo => tmyo}
import com.typesafe.scalalogging.StrictLogging

object Myo extends StrictLogging {

  type Myo = tmyo.Myo
  private type MyoQuat = tmyo.Quaternion

  // Angles is a datastructure and helper namespace for 3d rotation.
  case class Angles(x:Double,y:Double,z:Double) {
    def relativeTo(ref:Angles) = {
      def shortestRotation(angle:Double, refAngle:Double):Double = {
        val diff1 = angle - 2 * Math.PI - refAngle
        val diff2 = angle + 2 * Math.PI - refAngle
        val diff3 = angle - refAngle

        val abs1 = Math.abs(diff1)
        val abs2 = Math.abs(diff2)
        val abs3 = Math.abs(diff3)

        val min = Math.min(abs1, Math.min(abs2, abs3))

        if (min == abs1) return diff1
        if (min == abs2) return diff2
        else return diff3
      }
      Angles(
        shortestRotation(x,ref.x),
        shortestRotation(y,ref.y),
        shortestRotation(z,ref.z)
      )
    }
  }

  import gov.nasa.worldwind.geom.{Quaternion => WWQuat}

  object Angles {
    // compresses radian angles into range [-1 1], and applies deadzoning.
    def processed(radianAngle:Double):Double = {
      def compress(v:Double, max:Double) = v / max
      val deadzone = 0.1
      assert(deadzone < 1)
      val sign = if (radianAngle < 0) -1 else 1
      // range: [-2pi - +2pi]
      var v = compress(radianAngle, 2 * Math.PI)
      // range: [-1 - +1]
      // return 0 if angle within deadzone
      v = Math.abs(v) // absoluting
      v = if ((v - deadzone) >= 0) v else return 0
      // v = Math.min(v, 1) // clip if angle wasn't =< 2pi.
      assert(v <= 1) // better to assert, fail early
      // range: [deadzone - max]
      v = v - deadzone
      // range: [0 - (max - deadzone)]
      v = v / (1 - deadzone)
      // range: [0 - 1]
      // cube to reduce sensitivity
      v = Math.pow(v,3)
      // restore relativity.
      v = sign * v
      return v
    }
    // converts myo's quaternion into Angles processed with above method.
    def apply(myoQuat:MyoQuat):Angles = {
      val wwQuat =
        new WWQuat(
          myoQuat.getX, myoQuat.getY, myoQuat.getZ, myoQuat.getW)
      return Angles(
        processed(wwQuat.getRotationX.radians),
        processed(wwQuat.getRotationY.radians),
        processed(wwQuat.getRotationZ.radians))
    }
  }

  // breaking down tmyo.Pose to PoseType
  type Pose = tmyo.enums.PoseType

  // aliases the posetype enum to Poses.*
  object Poses {
    import tmyo.enums.{PoseType => PT}
    val REST = PT.REST
    val FIST = PT.FIST
    val FINGERS_SPREAD = PT.FINGERS_SPREAD
    val WAVE_IN = PT.WAVE_IN
    val WAVE_OUT = PT.WAVE_OUT
    val DOUBLE_TAP = PT.DOUBLE_TAP
    val UNKNOWN = PT.UNKNOWN
  }

  /*
  trait MyoEvent {
    val timestamp:Long
    val myo:Myo
  }

  case class PoseEvent(
    myo:Myo, timestamp:Long, pose:Pose) extends MyoEvent

  case class OrientationEvent(
    myo:Myo, timestamp:Long, angles:Angles) extends MyoEvent
    
  object OrientationEvent {
    def apply(myo:Myo, timestamp:Long, myoQuat:MyoQuat):OrientationEvent =
      // get rid of Quaternions as soon as possible, for the sake of simpler abstraction later on.
      // if performance were affected, could make angles a lazy field on OrientationEvent, and only use Quaternions for instantiation. that way conversion is postponed until is needed.
      OrientationEvent(myo, timestamp, Angles(myoQuat))
  }
  */

  import akka.stream.scaladsl.SourceQueue

  // listener to be supplied to myo-java. provides callbacks for event handling
  class Listener(
    orientationQ:SourceQueue[Angles], poseQ:SourceQueue[Pose]
  ) extends tmyo.AbstractDeviceListener {
    override def onPose(
      myo:Myo, timestamp:Long, tmyoPose:tmyo.Pose) {
        // strip down unnecessary tmyo.Pose object to PoseType
        val pose:Pose = tmyoPose.getType
        poseQ offer pose
    }
    override def onOrientationData(
      myo:Myo, timestamp:Long, myoQuat:MyoQuat) {
        orientationQ offer Angles(myoQuat)
    }
  }

  import java.util.concurrent.atomic.{AtomicReference => Atom}

  // thread-safe mutable option
  class OptionAtom[T](v:Option[T] = None) extends Atom(v) {
    def unset = this.set(None)
    // moving convenience .set(v:T) to value class, because type erasure
    // makes such implementations of generics impossible.
    //def set(v:T) = super.set(Some(v))
  }

  object OptionAtom {
    // convenience .set(v:T) to be able to set without doing .set(Some(...))
    implicit class Better[T](val self:OptionAtom[T]) extends AnyVal {
      def set(v:T):Unit = self.set(Some(v))
    }
  }

  object Extensions {
    implicit class BetterPose(val pose:Pose) extends AnyVal {
      // checks if drone command mapped to pose uses orientation data
      def usesOrientation: Boolean = {
        // data/code determining how poses are classed should be kept elsewhere
        // ideally with mapping data
        import Poses._
        pose match {
          case FIST | FINGERS_SPREAD => true
          case _ => false
        }
      }
    }
  }

  // all this persistence would ideally be implemented as cycles in the stream.
  val ReferenceAnglesAtom = new OptionAtom[Angles]
  val CurrentPose = new Atom(Poses.REST)
  val OnGround = new Atom(true)

  // Zips orientation and posture data into drone's wanted state.
  def zip(pose:Pose, curAngles:Angles):Drone.WantedState = {
    logger.trace(s"zipping $pose and $curAngles")
    val lastPose:Pose = CurrentPose.get
    val poseChanged = lastPose != pose
    logger.trace(s"poseChanged = $poseChanged")

    // Atom care
    if (poseChanged) {
      // if pose changed, there are atoms that need to be updated
      import Extensions._
      logger.trace(s"CurrentPose.set($pose)")
      // current pose changed
      CurrentPose.set(pose)
      if (pose.usesOrientation) {
        // pose is mapped to a command that requires reference orientation.
        logger.trace(s"ReferenceAnglesAtom.set($curAngles)")
        ReferenceAnglesAtom.set(curAngles)
      } else if (lastPose.usesOrientation) {
        // command mapped to last pose used orientation,
        // but current command doesn't need them.
        // therefore, need to drop reference angles.
        // this second test is an optimization to minimize atom uses,
        // .unset could just be called on all non-movement cases.
        logger.trace(s"ReferenceAnglesAtom.unset")
        ReferenceAnglesAtom.unset

      }
    }

    // this could be optimized away by using a local var to track
    // refAngles value.
    val relAngles = ReferenceAnglesAtom.get.map(curAngles.relativeTo(_))
    logger.trace(s"relAngles.isDefined = ${relAngles.isDefined}")

    import Drone.States
    import Poses._

    val DefaultState = States.Hovering

    relAngles match {
      case Some(relAngles) => pose match {
        // poses that require myo angles.
        case FIST => States.Tilting(relAngles)
        case FINGERS_SPREAD => States.ChangingAltitude(relAngles.y)
        case _ => DefaultState
      }
      case _ => pose match {
        // poses that don't require angles
        case WAVE_IN if poseChanged => {
          val onGround = OnGround.get
          OnGround.set(!onGround)
          if (onGround) States.TakingOff
          else States.Landing
        }
        //case WAVE_OUT => States.TakingOff
        //case WAVE_IN => States.Landing
        case _ => DefaultState
      }
    }
  }

  object Hub {
    import com.thalmic.myo.Hub

    val hub = new Hub("com.myoar.mainhub")

    def init(myoListener:Listener) = {
      val myo:Myo = hub.waitForMyo(10000)
      if (myo == null) {
        throw new RuntimeException(
          "hub.waitForMyo(10000) expired without finding Myo.")
      }
      hub.addListener(myoListener)
    }
  }

  // EventLoop.start
  val EventLoop = new Thread {
    override def run = while (true) Hub.hub.run(30)
  }

}
